import cv2
import numpy as np

# 相机内参矩阵
K = np.array([[465.89830246, 0.0, 321.75423926],
              [0.0, 465.70203766, 234.38544949],
              [0.0, 0.0, 1.0]])

# 畸变系数
dist_coeffs = np.array([-1.56124824e-03, 3.56434644e-01, -2.32354193e-04, -1.35109750e-03, -7.74506460e-01])

# 输入像素坐标（单位为像素）
x_pixel = 400
y_pixel = 200

print("畸变前像素坐标 (u, v):", x_pixel, y_pixel)

# 创建像素坐标的数组
pixel_coords = np.array([[x_pixel, y_pixel]], dtype=np.float32)

# 进行畸变校正
undistorted_coords = cv2.undistortPoints(pixel_coords, K, dist_coeffs)

# 提取校正后的像素坐标
x_undistorted = undistorted_coords[0, 0, 0]
y_undistorted = undistorted_coords[0, 0, 1]

print("校正后像素坐标 (u, v):", x_undistorted, y_undistorted)
